import numpy as np
import warnings
import copy
import rospy
from sklearn.cluster import KMeans

def node():
    # Your existing code here
    while not rospy.is_shutdown():
        # -------------------------------------------------------------------------    
        # Clustering frontier points
        centroids = []
        quantityfrontiers_recrod = []
        quantity_recrod = []
        front = copy(frontiers)

        quantityfrontiers_recrod.append(len(frontiers))
        rospy.loginfo("quantityfrontiers_recrod:" + str(quantityfrontiers_recrod))             
        quantity_recrod.append(len(front))
        rospy.loginfo("quantity_recrod:" + str(quantity_recrod))

        if len(front) > 1:
            # 替换为 KMeans 聚类
            kmeans = KMeans(n_clusters=3, max_iter=100, random_state=0)  # 根据需要调整 n_clusters
            kmeans.fit(front)
            centroids = kmeans.cluster_centers_  # 使用 KMeans 的聚类中心

        # 如果只有一个前沿点，无需聚类，即 centroids = frontiers
        if len(front) == 1:
            centroids = front
        frontiers = copy(centroids)

if __name__ == "__main__":
    node()